#include "stm32f10x.h"                  // Device header
#include "math.h"
#include "Delay.h"
#include "OLED.h"
#include "LED.h"
#include "Timer.h"
#include "Key.h"
#include "RP.h"
#include "Motor.h"
#include "Encoder.h"
#include "Serial.h"

#define EPSILON 10e-6
 
uint8_t KeyNum;

// PID 的全局变量
float Target, Actual, Out;
float Kp = 0.2, Ki = 0.2, Kd = 0;
float Error0, Error1, ErrorInt = 0;

void Motor_Test()
{
	OLED_Init();
	LED_Init();
	Key_Init();
	Motor_Init();
	Encoder_Init();
	Timer_Init();
	
	// 电位器: 用于修改 Kp, Ki, Kd 的值
	RP_Init();
	
	// 串口初始化
	Serial_Init();
	
	// 显示标题
	OLED_Printf(0, 0, OLED_8X16, "Location Control");
	OLED_Update();
	
	while(1)
	{		
		// 读取电位器的值来来修改 Kp, Ki, Kd
		// RP4 控制 Targets
		Kp     = RP_GetValue(1) / 4095.0 * 2;
		Ki     = RP_GetValue(2) / 4095.0 * 2;
		Kd     = RP_GetValue(3) / 4095.0 * 2;
		Target = RP_GetValue(4) / 4095.0 *816 - 408; 
		
		// 显示 Kp, Ki, Kd
		OLED_Printf(0, 16, OLED_8X16, "Kp:%4.2f", Kp);
		OLED_Printf(0, 32, OLED_8X16, "Ki:%4.2f", Ki);
		OLED_Printf(0, 48, OLED_8X16, "Kd:%4.2f", Kd);
		
		// 显示目标，测量，输出
		OLED_Printf(64, 16, OLED_8X16, "Tar:%+04.0f", Target);
		OLED_Printf(64, 32, OLED_8X16, "Act:%+04.0f", Actual);
		OLED_Printf(64, 48, OLED_8X16, "Out:%+04.0f", Out);
		
		OLED_Update();
		
		// 输出到 Serial_Port
		Serial_Printf("%f, %f, %f\r\n", Target, Actual, Out);
	}
}

void TIM1_UP_IRQHandler(void)
{
	static uint16_t count       = 0;
	static uint16_t ledCounter = 0;
	if (TIM_GetITStatus(TIM1, TIM_IT_Update) == SET)
	{	
		TIM_ClearITPendingBit(TIM1, TIM_IT_Update);
		
		// 40ms 周期测量电机速度
		count++;
		if (count >= 40)
		{
			count = 0;
/***************PID******************/
			Actual += Encoder_Get();
			
			// 误差传递
			Error1 = Error0;
			Error0 = Target - Actual;
			
			// 误差积分(累加), 当 Ki 未被设置为 0 时，再开始误差累加
			if (fabs(Ki) > EPSILON)
			{
				ErrorInt += Error0;
			}
			else
			{
				ErrorInt = 0;
			}
			
			// PID 计算
			Out = Kp * Error0 + Ki * ErrorInt + Kd * (Error0 - Error1);
			
			// 输出限幅, 因为 Motor_SetPWM 的输入范围是 -100, 100
			if ( Out > 100) { Out = 100;  }
			if ( Out < -100){ Out = -100; }
			
			// PID 控制器的输出应用到被控对象
			Motor_SetPWM(Out);
		}
		
		// OLED 灯闪烁
		if (ledCounter++ >= 250)
		{
			ledCounter = 0;
			LED_Turn();
		}
	}
}

int main(void)
{
	Motor_Test();
	while(1)
	{
	}
}
